#include <iostream>
#include <string>
#include <thread>
#include "../Lib/MiniBotAlgorithmLib.h"
using namespace std;

static int robotid = 0;
void (setPositionCallback)(unsigned char id, double angle) {
	cout << "setPosition" << id << " " << angle << endl;
}

void (setPositionWithSpeedCallback)(unsigned char id, double angle, int speedPersent)
{
	cout << "setPositionWithSpeed" << id << " " << angle << endl;
}

void (setMultiPositionCallback)(unsigned char* ids, double* angles, int count)
{
	cout << "setMultiPosition" << ids << " " << count << endl;
}

void (setMultiPositionWithSpeedCallback)(unsigned char* ids, double* angles, int count, int speedPersent)
{
	cout << "setMultiPositionWithSpeed" << ids << " " << count << endl;
}

void (setPositionSpeedCallback)(unsigned char id, int speedPersent)
{
	cout << "setPositionSpeedCallback" << id << " " << speedPersent << endl;
}

void (setRunningSpeedCallback)(unsigned char id, int speedPersent)
{
	cout << "setRunningSpeedCallback" << id << " " << speedPersent << endl;
}


void (setTorqueEnableCallback)(unsigned char id, bool isEnalbe)
{
	cout << "enableTorque" << id << " " << isEnalbe << endl;
}

void (setModeCallback)(unsigned char id, int mode)
{
	cout << "setMode" << id << " " << mode << endl;
}

void runSanke() {
	SendCmd(robotid, 0, 0);
}


/*
* 指定构型机器人添加与管理控制
*/
int Sample_RobotAddNRemoveNBindNSendCmd()
{
	/*
	id=mlib.AddRobot(2,[31,32,33,34,35])
mlib.BindRobotModuleApis(id,setPositionCallback,setPositionWithSpeedCallback,setMultiPositionCallback,setMultiPositionWithSpeedCallback,setPositionSpeedCallback,setRunningSpeedCallback,setTorqueEnableCallback,setModeCallback)

_thread.start_new_thread(runSanke)
while(1):
	str = input("请输入：")
	cmd=int(str)
	mlib.SendCmd(id,cmd, 0);


	*/


	unsigned char ids[] = { 20,66,88,20,80 };
	auto id = AddRobot(2, ids, 5);
	BindRobotModuleApis(id, setPositionCallback, setPositionWithSpeedCallback, setMultiPositionCallback, setMultiPositionWithSpeedCallback, setPositionSpeedCallback, setRunningSpeedCallback, setTorqueEnableCallback, setModeCallback);
	robotid = id;
	thread th1(runSanke);
	while (1) {
		cout << "Please input cmd:" << endl;
		int cmd;
		cin >> cmd;
		SendCmd(robotid, cmd, 0);
	}

	getchar();
	return 0;
}

